/*

Copyright (c) by Emil Valkov,
All rights reserved.

License: http://www.opensource.org/licenses/bsd-license.php

*/

#include <cv.h>
#include <highgui.h>
#include <opencv2/opencv.hpp>

#include "RaspiCamCV.h"

#include <stdio.h>
#include <ctime>
#include <iostream>
#include <unistd.h>
#include <sys/time.h>

using namespace std;
using namespace cv;


// Important Consts
const int RED 	= 0;
const int GREEN = 1;
const int BLUE  = 2;



/*
Intput:	rStart, rStop - the start row and stop row to average over
	cStart, cStop - the start col and stop col to average over
	image - the image to average

Output: the average over the region commanded for:
	red 	- average[0]
	green	- average[1]
	blue 	- average[2]

*/
inline int * averageOverRange(const Mat &image, int rStart, int rStop, int cStart, int cStop){
	// TODO: Make sure this can handle bad calls (i.e. errors)	
	long * average = new long[3];
	
	// TODO: Make sure I don't run outside of the image and cause segfaults
	
	// Sum up all numbers over given range for Red, Green, and Blue
	for (long i = rStart; i < rStop; i++){
		for (long j = cStart; j < cStop; j++){
			average[RED] 	+= image.at<Vec3b>(i,j)[RED];
			average[GREEN] 	+= image.at<Vec3b>(i,j)[GREEN];
			average[BLUE] 	+= image.at<Vec3b>(i,j)[BLUE];
		}
	}
	
	// Divide by range size to finish computing the average
	average[RED] 	/= (rStop - rStart)*(cStop - cStart);
	average[GREEN] 	/= (rStop - rStart)*(cStop - cStart);
	average[BLUE] 	/= (rStop - rStart)*(cStop - cStart);
	return average;
}

inline int ** computeBlockAverages(const Mat &image,int numBlocks){
 
	// TODO: Make sure image is not null or of size < numBlocks and that numBlocks != 0
	int ** average = new int*[numBlocks];
	// divide image into blocks
	int rowBlockSize = image.rows / numBlocks;
	int colBlockSize = image.cols / numBlocks;
	
	for (int i = 0; i < numBlocks; i++){
		
		// Compute averages over blocks		
		//int * temp;
		average[i] = averageOverRange(image,
			i * rowBlockSize, (i+1) * rowBlockSize,
			i * colBlockSize, (i+1) * colBlockSize);
		
		// Perform copy
		//average[i][RED] = temp[RED];	
		//average[i][GREEN] = temp[GREEN];
		//average[i][BLUE] = temp[BLUE];
		//average[i] = temp;
		// Cleanup 
		//delete temp;
	
	}
	return average;
	

}

int main(int argc, const char** argv){
	RaspiCamCvCapture * capture = raspiCamCvCreateCameraCapture(0); // Index doesn't really matter
	cvNamedWindow("RaspiCamTest", 1);
	Mat image; 
	//IplImage * temp;
	timeval a;
	timeval b;

	uchar * ptr = NULL;
	uchar * out = new uchar[8];

	gettimeofday(&a,0);
	bool condition = true;
	int ** average;
	image = Mat(raspiCamCvQueryFrame(capture));
	
	while(condition){
		image = Mat(raspiCamCvQueryFrame(capture));
		//printf("Width %d, height %d\n",image->width,image->height);
		imshow("RaspiCamTest",image);
		// cvReleaseImage(&image);
		cvWaitKey(0);
		ptr = image.ptr(0);
		cout << "Row Col Chan: " << image.rows <<' '<< image.cols << ' ' << image.channels()<< endl;
		
		int numBlocks = 9;
		average = computeBlockAverages(image,numBlocks);
		
		for (int i = 0; i < numBlocks; i++){
			cout << "Block " 	<< i+1 << ":  " << endl;				
			cout << "Avg Red:   " 	<< average[i][RED] << endl;
			cout << "Avg Green: " 	<< average[i][GREEN] << endl;
			cout << "Avg Blue:  " 	<< average[i][BLUE] << endl;
			
		}
		delete average;
		

		//condition = false;
		  
		  
	}
	gettimeofday(&b,0);

	cout << "diff: " << (b.tv_sec - a.tv_sec) << endl;



	destroyWindow("RaspiCamTest");
	//cvReleaseImage(&image);
	raspiCamCvReleaseCapture(&capture);
	return 0;
}
